
#include <BSphere>
#include "IContinuousConvexCast.h"
#include "ISphereSphereAlgorithm.h"
#include "member_BNode.h"
#include "member_BBody.h"

using namespace BWE;

ISphereSphereAlgorithm::ISphereSphereAlgorithm()
{

}
ISphereSphereAlgorithm::~ISphereSphereAlgorithm()
{

}

void ISphereSphereAlgorithm::caculate()
{
	BSphere* sphere0 = (BSphere*)_shapeA;
	BSphere* sphere1 = (BSphere*)_shapeB;

	BVector diff = _matrixA.position() - _matrixB.position();
	BReal len = diff.length();
	BReal radius0 = sphere0->radius();
	BReal radius1 = sphere1->radius();
	if (len > radius0 + radius1)
		return;

	BVector normalB(1, 0, 0);
	if (len > REAL_EPSILON)
		normalB = diff / len;

	BVector pointB = _matrixB.position() + normalB * radius1;

	BReal distance = len - (radius0 + radius1);
	this->addContactPoint(pointB, normalB, distance);
}
void ISphereSphereAlgorithm::timeImpact()
{
	BSphere* sphere0 = (BSphere*)_shapeA;
	BSphere* sphere1 = (BSphere*)_shapeB;

	const BMatrix& matrixA = node_member(_bodyA)->matrix;
	const BMatrix& matrixB = node_member(_bodyB)->matrix;

	const BMatrix& interpMatrixA = body_member(_bodyA)->predictedMatrix;
	const BMatrix& interpMatrixB = body_member(_bodyB)->predictedMatrix;

	CastResult caseResult;
	IContinuousConvexCast ccc(_shapeA, _shapeB);
	ccc.calcTimeOfImpact(matrixA, interpMatrixA, matrixB, interpMatrixB, caseResult);
}
